from pointMass import PointMass
from vec3 import Vec3

class Spring:
    def __init__(self, fromPoint, toPoint, length = None, stiffness = 1, friction = 0.25):
        self.fromPoint = fromPoint
        self.toPoint = toPoint
        self.length = length or (fromPoint.pos - toPoint.pos).length()
        self.stiffness = stiffness
        self.friction = friction
        self.broken = False

    def tick(self, dt):
        diff = self.toPoint.pos - self.fromPoint.pos
        
        if abs(diff.length() - self.length) / self.length > 1:
            self.broken = True
        
        springForce = diff.norm() * ((diff.length() - self.length) * self.stiffness)
        self.fromPoint.applyForce(springForce)
        self.toPoint.applyForce(-springForce)

        fromRelativeVel = self.fromPoint.vel - self.toPoint.vel
        fromProjectedVel = (-diff).project(fromRelativeVel)
        fromFrictionForce = fromProjectedVel * -self.friction
        self.fromPoint.applyForce(fromFrictionForce)

        toRelativeVel = self.toPoint.vel - self.fromPoint.vel
        toProjectedVel = diff.project(toRelativeVel)
        toFrictionForce = toProjectedVel * -self.friction
        self.toPoint.applyForce(toFrictionForce)

if __name__ == "__main__":
    fromPoint = PointMass()
    toPoint = PointMass(pos = Vec3(2,0,0))
    spring = Spring(fromPoint,toPoint, length = 1)
    import time
    step = 0
    dt = 0.01
    while True:
        step += dt
        spring.tick(dt)
        fromPoint.tick(dt)
        toPoint.tick(dt)
        print fromPoint.pos, toPoint.pos, step
        time.sleep(dt)
